//
// Created by fanghr on 17-12-31.
//

#ifndef PROJECT_SHM_DATA_H
#define PROJECT_SHM_DATA_H

#include "global_var.h"
#include "shm_pack.h"
#include "../include.h"
#include "../sharedmemory/shm.h"

void thread_shm_data() {
    while (true) {
        imu_global::shm_pack.shm_imu_angle.lock();
        imu_global::imu_angle_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_angle.get(), &imu_global::imu_angle_pack.data, sizeof(imu_angle));
        imu_global::shm_pack.shm_imu_angle.unlock();
        imu_global::imu_angle_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_angularVelocity.lock();
        imu_global::imu_angularVelocity_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_angularVelocity.get(), &imu_global::imu_angularVelocity_pack.data,
               sizeof(imu_angularVelocity));
        imu_global::shm_pack.shm_imu_angularVelocity.unlock();
        imu_global::imu_angularVelocity_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_acceleration.lock();
        imu_global::imu_acceleration_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_acceleration.get(), &imu_global::imu_acceleration_pack.data,
               sizeof(imu_acceleration));
        imu_global::shm_pack.shm_imu_acceleration.unlock();
        imu_global::imu_acceleration_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_groundSpeed.lock();
        imu_global::imu_groundSpeed_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_groundSpeed.get(), &imu_global::imu_groundSpeed_pack.data,
               sizeof(imu_groundSpeed));
        imu_global::shm_pack.shm_imu_groundSpeed.unlock();
        imu_global::imu_groundSpeed_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_latitudeAndLongitudeOutput.lock();
        imu_global::imu_latitudeAndLongitudeOutput_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_latitudeAndLongitudeOutput.get(),
               &imu_global::imu_latitudeAndLongitudeOutput_pack.data,
               sizeof(imu_latitudeAndLongitudeOutput));
        imu_global::shm_pack.shm_imu_latitudeAndLongitudeOutput.unlock();
        imu_global::imu_latitudeAndLongitudeOutput_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_magneticOutput.lock();
        imu_global::imu_magneticOutput_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_magneticOutput.get(), &imu_global::imu_magneticOutput_pack.data,
               sizeof(imu_magneticOutput));
        imu_global::shm_pack.shm_imu_magneticOutput.unlock();
        imu_global::imu_magneticOutput_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_portStatus.lock();
        imu_global::imu_portStatus_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_portStatus.get(), &imu_global::imu_portStatus_pack.data,
               sizeof(imu_portStatus));
        imu_global::shm_pack.shm_imu_portStatus.unlock();
        imu_global::imu_portStatus_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_pressureAndHeightOutput.lock();
        imu_global::imu_pressureAndHeightOutput_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_pressureAndHeightOutput.get(),
               &imu_global::imu_pressureAndHeightOutput_pack.data,
               sizeof(imu_pressureAndHeightOutput));
        imu_global::shm_pack.shm_imu_pressureAndHeightOutput.unlock();
        imu_global::imu_pressureAndHeightOutput_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_quaternion.lock();
        imu_global::imu_quaternion_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_quaternion.get(), &imu_global::imu_quaternion_pack.data,
               sizeof(imu_quaternion));
        imu_global::shm_pack.shm_imu_quaternion.unlock();
        imu_global::imu_quaternion_pack.lock.unlock_shared();

        imu_global::shm_pack.shm_imu_time.lock();
        imu_global::imu_time_pack.lock.lock_shared();
        memcpy(imu_global::shm_pack.shm_imu_time.get(), &imu_global::imu_time_pack.data,
               sizeof(imu_time));
        imu_global::shm_pack.shm_imu_time.unlock();
        imu_global::imu_time_pack.lock.unlock_shared();
    }
}

#endif //PROJECT_SHM_DATA_H
